#include "stdafx.h"
#include "cvxConcentric.h"
#include "cvxMarker.h"
#include "cvxMat.h"
#include "cvxSf.h"
using cvx::RgbImage;
using cvx::BwImage;


bool CvxConcentric::detect(IplImage *grayImage, const Point2f &seedPt, 
						   Point2f &p, float &ratio, bool fixRatio, 
						   const float inRatio, const float ratioThreshold)
{ 
	assert(grayImage && grayImage->nChannels == 1);

#define CVXCONCENTRIC_DETECT 0

	if (seedPt.x < 0 || seedPt.y <0 ||seedPt.x >= grayImage->width || seedPt.y >= grayImage->height)
	{
		return false;
	}
	p = seedPt;

#if CVXCONCENTRIC_DETECT 
	IplImage *testImage = cvCreateImage(cvGetSize(grayImage), IPL_DEPTH_8U, 3);
	cvCvtColor(grayImage, testImage, CV_GRAY2BGR);
#endif

	//get 1D points
	int degree = 0;
	int fsize = 9;
	int offset = fsize/2;
	float sigma = fsize/6.0f;
	double d_kernel[9];
	Mat kernel = Mat(1, 9, CV_64F, d_kernel);
	for (int i = 0; i<fsize; ++i)
	{
		kernel.at<double>(0, i) = (i-offset) / (sigma * sigma) * exp(-(i-offset)*(i-offset)/(2.0*sigma*sigma));  
	}
	cv::flip(kernel, kernel, 1);	

	float min_ratio = INT_MAX;
	Point2f min_center = seedPt;
	float pre_ratio = 0.0;
	for (int i = 0; i<10; ++i)
	{
	//	printf("i = %d\n", i);
		
		if (i == 8)
		{
			int test = 1;
		}
		//points on concentric circles and center
		Point2f pa, pb, pc, pd;
		Point2f pcenter;

		bool xDominate = false;
		double startXY = 0.0;
		double slop = 0;

		vector<unsigned char> lineDataVec;
		bool found = GetLinePoints(grayImage, p, degree, xDominate, startXY, slop, lineDataVec);
		if (!found)
		{
			fprintf(stderr, "not found lines!\n");
			return false;
		}
		Mat lineDataMat(1, lineDataVec.size(), CV_64F);
		for (int j = 0; j<lineDataVec.size(); ++j)
		{
			lineDataMat.at<double>(0, j) = lineDataVec[j];
		}

		//convolution with gaussian	
		cv::filter2D(lineDataMat, lineDataMat, -1, kernel, Point(4, 0), 0, BORDER_DEFAULT);

		//get 4 max min points
		if (xDominate)
		{
			//middle point index in the lindeDataMat
			int midPx = cvRound(p.x) - cvRound(startXY);
			if (midPx <= 0 && midPx >= lineDataMat.cols)
			{
				return false;
			}
			Mat leftMat  = lineDataMat.colRange(0, midPx);
			Mat rightMat = lineDataMat.colRange(midPx, lineDataMat.cols);

			//a line cross concentric circles in pa, pb, pc, pd			
			double minPos, maxPos; 
			minMaxIdx1DSubPixel(leftMat,  minPos, maxPos);
			pa.x = startXY + maxPos;
			pa.y = slop * (pa.x - p.x) + p.y;
			pb.x = startXY + minPos;
			pb.y = slop * (pb.x - p.x) + p.y;

			minMaxIdx1DSubPixel(rightMat, minPos, maxPos);
			minPos += (p.x - startXY);
			maxPos += (p.x - startXY);
			pc.x = startXY + maxPos;
			pc.y = slop * (pc.x - p.x) + p.y;
			pd.x = startXY + minPos;
			pd.y = slop * (pd.x - p.x) + p.y;


			//find concentric circles center
			double midPosX = 0;			
			bool isInside = GetMiddlePoints(pa.x, pb.x, pc.x, pd.x, midPosX, ratio);
			if (isInside)
			{
				pcenter.x = midPosX;
				pcenter.y = slop * (pcenter.x - p.x) + p.y;
			}
			else
			{
				return false;
			}
			
		}
		else
		{
			//middle point index according to y coordinate
			int midPy = cvRound(p.y) - cvRound(startXY);
			if (midPy <= 0 && midPy >= lineDataMat.cols)
			{
				return false;
			}
			Mat leftMat  = lineDataMat.colRange(0, midPy);
			Mat rightMat = lineDataMat.colRange(midPy, lineDataMat.cols);

			//a line cross concentric circles in pa, pb, pc, pd

			double minPos, maxPos; 
			minMaxIdx1DSubPixel(leftMat,  minPos, maxPos);
			pa.y = startXY + maxPos;
			pa.x = slop * (pa.y - p.y) + p.x;
			pb.y = startXY + minPos;
			pb.x = slop * (pb.y - p.y) + p.x;

			minMaxIdx1DSubPixel(rightMat, minPos, maxPos);
			minPos += (p.y - startXY);
			maxPos += (p.y - startXY);
			pc.y = startXY + maxPos;
			pc.x = slop * (pc.y - p.y) + p.x;
			pd.y = startXY + minPos;
			pd.x = slop * (pd.y - p.y) + p.x;


			//find concentric circles center
			double midPosY = 0;			
			bool isInside = GetMiddlePoints(pa.y, pb.y, pc.y, pd.y, midPosY, ratio);
			if (isInside)
			{
				pcenter.y = midPosY;
				pcenter.x = slop * (pcenter.y - p.y) + p.x;
			}
			else
			{
				return false;
			}			
		}

#if CVXCONCENTRIC_DETECT 
		cvCvtColor(grayImage, testImage, CV_GRAY2BGR);
	//	cv::line(Mat(testImage), cvPoint(pa.x, pa.y), cvPoint(pd.x, pd.y), cvScalar(0, 0, 255));
		cv::circle(Mat(testImage), cvPoint(pa.x, pa.y), 1, cvScalar(255, 0, 0));
		cv::circle(Mat(testImage), cvPoint(pb.x, pb.y), 1, cvScalar(0, 0, 255));
		cv::circle(Mat(testImage), cvPoint(pc.x, pc.y), 1, cvScalar(255, 255, 255));
		cv::circle(Mat(testImage), cvPoint(pd.x, pd.y), 1, cvScalar(0, 255, 0));
		
	//	cv::circle(Mat(testImage), cvPoint(pcenter.x, pcenter.y), 1, cvScalar(0, 0, 255));
		cvShowImage("line", testImage);
		cvWaitKey(10);
#endif
		
		p = pcenter;
		degree += 80;
	//	fprintf(stdout, "p %f %f\n", p.x, p.y);


		if (fixRatio)
		{
			if (fabs(ratio - inRatio) < fabs(min_ratio - inRatio))
			{
				min_ratio = ratio;
				min_center = p;	
				if (fabs(min_ratio - inRatio) < ratioThreshold)
				{
					return true;
				}
			}			
		}
		else if (pre_ratio > ratio)
		{
			return true;
		}
		pre_ratio = ratio;
	}
	ratio = min_ratio;
	p = min_center;
#if CVXCONCENTRIC_DETECT 
	cvReleaseImage(&testImage);
#endif
	if (fabs(min_ratio - inRatio) < 2 * ratioThreshold)
	{
		return true;
	}
	else
	{
		return false;
	}
			
}


//sita is line slop with unit in degree. set 80 
bool CvxConcentric::GetLinePoints(IplImage *grayImage, const Point2f &midP, int sita,
								  bool &xDominate, double &startXorY, double &lineSlop,
								  vector<unsigned char> & lineData)
{
	assert(grayImage);
	assert(grayImage->nChannels == 1);

	int w = grayImage->width;
	int h = grayImage->height;
	float mx = midP.x, my = midP.y;
	//middle point out of image 
	if (mx<0 || mx>=w ||my<0 ||my>=h)
	{
		return false;
	}
	sita = sita%180;
	lineData.clear();

	BwImage g_img(grayImage);
	//x0, y0, x1, y1 is points pairs that line intersect with image border
	double x0, y0, x1, y1;	
	double s = tan(sita/180.0*CV_PI);
	double yL = s * (0.0-mx)+ my;

	if (cvRound(yL)<0.0)
	{
		y0 = 0.0;
		x0 = 1.0/s*(y0-my) + mx;
	}
	else if (cvRound(yL)>=h)
	{
		y0 = h-1;
		x0 = 1.0/s*(y0-my) + mx;
	}
	else
	{
		y0 = yL;
		x0 = 0.0;
	}

	double yR = s*(w-1.0-mx) + my;
	if (cvRound(yR)<0.0)
	{
		y1 = 0.0;
		x1 = 1.0/s*(y1-my) + mx;
	}
	else if (cvRound(yR)>=h)
	{
		y1 = h-1;
		x1 = 1.0/s*(y1-my) + mx;
	}
	else
	{
		y1 = yR;
		x1 = w-1.0;
	}

	//x dominated
	if (sita<=45 || sita>=135)
	{
		xDominate = true;
		if (x1<x0)
		{
			std::swap(x0, x1);
			std::swap(y0, y1);
		}
		startXorY = x0;
		int xs = cvRound(x0), xe = cvRound(x1);	
		for (int x = xs; x<xe; ++x)
		{
			int y = cvRound(s * (x - mx) + my);
			lineData.push_back(g_img[y][x]);
		}
		lineSlop = s;
		return true;
	}
	else
	{
		xDominate = false;
		s = 1.0/s;
		if (y1 < y0)
		{
			std::swap(x0, x1);
			std::swap(y0, y1);			
		}
		startXorY = y0;
		int ys = cvRound(y0), ye = cvRound(y1);
		for (int y = ys; y<ye; ++y)
		{
			int x = cvRound(s * (y - my) + mx);
			lineData.push_back(g_img[y][x]);
		}
		lineSlop = s;
		return true;
	}
}

//get subpixel position of max, min value Index from 1 D array,
//assume it just has one max min value in the array
void CvxConcentric::minMaxIdx1DSubPixel(const Mat &inMat, double &minPos, double &maxPos)
{
#define CVXCONCENTRIC_EPS 0.000001
	assert(inMat.rows == 1);
	//max1p
	double v_min, v_max, v_left, v_right;
	int idx_min = 0, idx_max = 0;
	cv::minMaxIdx(inMat, &v_min, &v_max, NULL, NULL);
	for (int i = 0; i<inMat.cols; ++i)
	{
		if (inMat.at<double>(0, i) == v_max)
		{
			idx_max = i;
		}
		if (inMat.at<double>(0, i) == v_min)
		{
			idx_min = i;
		}
	} 		
	v_left = v_right = v_max;
	if (idx_max > 0)
	{
		v_left  = inMat.at<double>(0, idx_max-1);			
	}
	if (idx_max + 1< inMat.cols)
	{
		v_right = inMat.at<double>(0, idx_max+1);
	}
	v_left = fabs(v_left) + CVXCONCENTRIC_EPS;
	v_max = fabs(v_max) + CVXCONCENTRIC_EPS;
	v_right = fabs(v_right) + CVXCONCENTRIC_EPS;

	double subpixel = 0.5*(log(v_left)-log(v_right))/(log(v_left)-2.0*log(v_max)+log(v_right));
	maxPos = idx_max + subpixel;

//	cvx::TestMat_d d_in(&inMat);
	//min1p
	v_left = v_right = v_min;
	if (idx_min > 0)
	{
		v_left  = inMat.at<double>(0, idx_min-1);			
	}
	if (idx_min + 1< inMat.cols)
	{
		v_right = inMat.at<double>(0, idx_min+1);
	}
	v_left = fabs(v_left)+ CVXCONCENTRIC_EPS;
	v_min = fabs(v_min) + CVXCONCENTRIC_EPS;
	v_right = fabs(v_right) + CVXCONCENTRIC_EPS;
	subpixel = 0.5*(log(v_left)-log(v_right))/(log(v_left)-2.0*log(v_min)+log(v_right));
	minPos = idx_min + subpixel;
}

bool CvxConcentric::GetMiddlePoints(const double leftMaxp, const double leftMinp, 
					 const double rightMaxp, const double rightMinp, 							 
					 double &midPx, float &ratio)
{
	double a = leftMaxp;
	double b = rightMinp;
	double c = leftMinp;
	double d = rightMaxp;
	double A = a+b-c-d;
	if (A != 0.0)
	{
		double B = -2.0*(a*b - c*d)/A;
		double C = (a*b*c + a*b*d - a*c*d - b*c*d)/A;
		double delta = B*B-4.0*C;
		if ( delta < 0.0)
		{
			return false;
		}
		delta = sqrt(delta);
		double x1 = (-B+delta)*0.5;
		double x2 = (-B-delta)*0.5;
		if (((x1 > a) && (x1<d)) || ((x1 >d) && (x1<a)))
		{
			midPx = x1;
			double r1 = ((midPx - c)/(midPx - a))/((x2 - c)/(x2 - a));
			double r2 = ((midPx - d)/(midPx - b))/((x2 - d)/(x2 - b));
			ratio = 0.5 *(r1+r2);
		}
		else
		{
			midPx = x2;
			double r1 = ((midPx - c)/(midPx - a))/((x1 - c)/(x1 - a));
			double r2 = ((midPx - d)/(midPx - b))/((x1 - d)/(x1 - b));
			ratio = 0.5 *(r1+r2);
		}
	}
	else
	{
		midPx = 0.5*(a + b);
		ratio = 0.5 * ((midPx - c)/(midPx - a) + (midPx - d)/(midPx-b));
	}
	return true;
}

ConcentriCheckerBoard::ConcentriCheckerBoard(CvSize size, CvSize markerSize, CvSize markerOffset, float unitLength)
{
	_inCircleRatio = 0.75/1.75;
	_outCircleRatio = 1.5/1.75;
	_size = size;
	_unitLength = unitLength;

	float p_offset = unitLength / 2.0; 

	_centerPts.resize(_size.width * _size.height);
	for (int y = 0; y<_size.height; ++y)
	{
		for (int x = 0; x<_size.width; ++x)
		{
			int idx = y * _size.width + x;

			//set center points
			Point2f pc;
			pc.x = x * unitLength;
			pc.y = y * unitLength;
			_centerPts[idx] = pc;

			//set boundary points;
			vector<Point2f> p_bdy(4);
			p_bdy[0].x = pc.x - p_offset;
			p_bdy[0].y = pc.y - p_offset;
			p_bdy[1].x = pc.x + p_offset;
			p_bdy[1].y = pc.y - p_offset;
			p_bdy[2].x = pc.x + p_offset;
			p_bdy[2].y = pc.y + p_offset;
			p_bdy[3].x = pc.x - p_offset;
			p_bdy[3].y = pc.y + p_offset;
			_boundrayPts.push_back(p_bdy);

			//set seed points
			Point2f pSeed;
			pSeed.x = pc.x;
			pSeed.y = pc.y - unitLength * _inCircleRatio/4.0;
			_seedPts.push_back(pSeed);
		}
	}



	//set marker position
	_markerPos.resize(4);
	_markerPos[0].x = 0.0f;
	_markerPos[0].y = 0.0f;
	_markerPos[1].x = unitLength * markerSize.width;
	_markerPos[1].y = 0.0f;
	_markerPos[2].x = unitLength * markerSize.width;
	_markerPos[2].y = unitLength * markerSize.height;
	_markerPos[3].x = 0.0f;
	_markerPos[3].y = unitLength * markerSize.height;
	for (int i = 0; i<4; ++i)
	{
		_markerPos[i].x += markerOffset.width  * unitLength;
		_markerPos[i].y += markerOffset.height * unitLength;
	}

	//marker corners
	float markerCornerOff = _outCircleRatio * unitLength * 0.5;
	for (int i = 0; i<4; ++i)
	{
		Point2f p = _markerPos[i];
		Point2f p1, p2, p3, p4;
		p1.x = p.x - markerCornerOff;
		p1.y = p.y - markerCornerOff;
		p2.x = p.x + markerCornerOff;
		p2.y = p.y - markerCornerOff;
		p3.x = p.x + markerCornerOff;
		p3.y = p.y + markerCornerOff;
		p4.x = p.x - markerCornerOff;
		p4.y = p.y + markerCornerOff;
		_markerCorners.push_back(p1);
		_markerCorners.push_back(p2);
		_markerCorners.push_back(p3);
		_markerCorners.push_back(p4);
	}
}

void ConcentriCheckerBoard::HomographyMap(const Mat &H, vector<Point2f> &centerPts,	
										  vector<Point2f> &seedPts, vector<vector<Point2f>> &boundary)const

{
	centerPts.clear();
	seedPts.clear();
	boundary.clear();

	int w = _size.width;
	int h = _size.height;
	double *pData = (double*)(H.data);
	centerPts.resize(w * h);
	seedPts.resize(w * h);
	boundary.resize(w * h);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			int idx = y * w + x;
			Point2f p1 = _centerPts[idx];
			Point2f p2;
			double scale = 1.0;
			p2.x  = pData[0] * p1.x  + pData[1] * p1.y + pData[2];
			p2.y  = pData[3] * p1.x  + pData[4] * p1.y + pData[5];
			scale = pData[6] * p1.x  + pData[7] * p1.y + pData[8];
			p2.x /= scale;
			p2.y /= scale;
			centerPts[idx] = p2;

			p1 = _seedPts[idx];
			p2.x  = pData[0] * p1.x  + pData[1] * p1.y + pData[2];
			p2.y  = pData[3] * p1.x  + pData[4] * p1.y + pData[5];
			scale = pData[6] * p1.x  + pData[7] * p1.y + pData[8];
			p2.x /= scale;
			p2.y /= scale;
			seedPts[idx] = p2;

			vector<Point2f> bdyPts(4);
			for (int i = 0; i<4; ++i)
			{
				p1 = _boundrayPts[idx][i];
				p2.x  = pData[0] * p1.x  + pData[1] * p1.y + pData[2];
				p2.y  = pData[3] * p1.x  + pData[4] * p1.y + pData[5];
				scale = pData[6] * p1.x  + pData[7] * p1.y + pData[8];
				p2.x /= scale;
				p2.y /= scale;
				bdyPts[i] = p2;
			}
			boundary[idx] = bdyPts;
		}
	}
}


bool cvxFindConcentriBoard(IplImage * grayImage, const CvSize size, const CvSize markerSize, 
						   const CvSize markerOffset, const float unitLength, vector<Point2f> &pts,
						   vector<bool> &mask,
						   bool useMarkerTracking,
						   const vector<CvxMarker> & preMarkers,
						   vector<CvxMarker> &curMarkers)
{
#define CVXFINDCONCENTRIBOARD_DEBUG 0

	//check parameters
	assert(grayImage);
	assert(grayImage->nChannels == 1);	

#if CVXFINDCONCENTRIBOARD_DEBUG
	IplImage *testImage = cvCreateImage(cvGetSize(grayImage), IPL_DEPTH_8U, 3);
	cvCvtColor(grayImage, testImage, CV_GRAY2BGR);
#endif
	//find markers
	vector<CvxMarker> detectedMarkers;
	MarkerCriteria criteria;
	criteria._minPixels = 100;
	criteria._maxPixels = 800;
	criteria._fastCheck = true;
	int ret = CVX_MARKER_DIF_NUMBER;
	if ((!useMarkerTracking) || preMarkers.size() != 4)
	{
		int markerNumber = 4;
	//	double tt = clock();
		ret = CvxMarker::detect(grayImage, detectedMarkers, markerNumber, criteria);
	//	printf("marker detection cost time = %f\n", clock() - tt);
	}
	else if(useMarkerTracking && preMarkers.size() == 4)
	{
		for (int i = 0; i<4; ++i)
		{
			CvRect r = preMarkers[i].m_subArea;
			IplImage *subImage = cvCreateImage(cvSize(r.width, r.height), IPL_DEPTH_8U, 1);
			cvSetImageROI(grayImage, r);
			cvCopy(grayImage, subImage);
			cvResetImageROI(grayImage);

			vector<CvxMarker> subMarker;
			
			int rst = CvxMarker::detect(subImage, subMarker, 1, criteria);			

			if (rst == CVX_MARKER_SUCCESS)
			{
				subMarker[0].AddOffset(r.x, r.y);
				detectedMarkers.push_back(subMarker[0]);
			}
			else
			{
				fprintf(stdout, "tracking marker in sub image failed\n");
				cvReleaseImage(&subImage);
				detectedMarkers.clear();
				return false;
			}
			cvReleaseImage(&subImage);
		}
		ret = CVX_MARKER_SUCCESS;
	}
	else
	{
		fprintf(stdout, "unexpected condition in concentric circles detection\n");
		return false;
	}

	if (ret !=CVX_MARKER_SUCCESS)
	{
		fprintf(stdout, "tracking marker failed\n");
		return false;
	}

	//get current marker sub area
	curMarkers.clear();
	for (int i = 0; i<4; ++i)
	{
		detectedMarkers[i].GetCenterPts();
		detectedMarkers[i].GetSubArea(10, grayImage->width, grayImage->height);
		curMarkers.push_back(detectedMarkers[i]);
	}

	//positions mapped in the image
	vector<Point2f> markerCenter;
	vector<Point2f> centerPos;
	vector<Point2f> seedPos;
	vector<vector<Point2f>> boundaryPos;

	//computer circles centers and seed seed points using homography
	for (int i = 0; i<4; ++i)
	{
		detectedMarkers[i].GetCenterPts();		
		markerCenter.push_back(detectedMarkers[i].m_centerPt);
	}

	ConcentriCheckerBoard checkerBoard(size, markerSize, markerOffset, unitLength);
	Mat Homo = CvxSF::findHomography4Pts(checkerBoard._markerPos, markerCenter);
	checkerBoard.HomographyMap(Homo, centerPos, seedPos, boundaryPos);

#if CVXFINDCONCENTRIBOARD_DEBUG
	
	//show center pts
	for (int i = 0; i<centerPos.size(); ++i)
	{
		cv::circle(Mat(testImage), cvPoint(centerPos[i].x, centerPos[i].y), 1.0, cvScalar(0, 0, 255));
	}
	cvShowImage("circles", testImage);
//	cvSaveImage("circles.png", testImage);
	cvWaitKey(0);
	//show seed pts
	cvCvtColor(grayImage, testImage, CV_GRAY2BGR);
	//show boundary pts

#endif


	//computer sub-area using circles boundary	
	BwImage g_img(grayImage);
	pts.resize(checkerBoard._size.height * checkerBoard._size.width);
	mask.resize(checkerBoard._size.height * checkerBoard._size.width);

	int w = grayImage->width;
	int h = grayImage->height;
	for (int y = 0; y<checkerBoard._size.height; ++y)
	{
		for (int x = 0; x<checkerBoard._size.width; ++x)
		{
			int idx = y * checkerBoard._size.width + x;
#if CVXFINDCONCENTRIBOARD_DEBUG
			printf("idx = %d\n", idx);
			if (idx == 130)
			{
				int test = 0;
			}
#endif
			if((y == markerOffset.height || y == markerOffset.height + markerSize.height) 
			    && (x == markerOffset.width || x == markerOffset.width + markerSize.width))
			{
				mask[idx] = false;
				pts[idx].x = -1.0f;
				pts[idx].y = -1.0f;
				continue;
			}		
			
			CvPoint p1 = cvPoint(INT_MAX, INT_MAX);
			CvPoint p2 = cvPoint(INT_MIN, INT_MIN);

			bool outImage = false;  //boundary out image
			for (int i = 0; i<4; ++i)
			{
				int bpx = cvRound(boundaryPos[idx][i].x);
				int bpy = cvRound(boundaryPos[idx][i].y);

				if (bpx >= 0 && bpx < w && bpy>=0 && bpy <h)
				{
					p1.x = std::min(p1.x, bpx);
					p1.y = std::min(p1.y, bpy);
					p2.x = std::max(p2.x, bpx);
					p2.y = std::max(p2.y, bpy);	
				}
				else
				{
					outImage = true;
					break;
				}								
			}
			if (outImage)
			{
				mask[idx] = false;
				pts[idx].x = -1.0f;
				pts[idx].y = -1.0f;
				continue;
			}

			CvRect r = cvRect(p1.x, p1.y, p2.x - p1.x, p2.y - p1.y);

			//get subImage with only one concentric circle
			IplImage *subImage = cvCreateImage(cvSize(r.width, r.height), IPL_DEPTH_8U, 1);
			cvSet(subImage, cvScalarAll(g_img[cvRound(seedPos[idx].y)][cvRound(seedPos[idx].x)]));
			IplImage *maskImage = cvCloneImage(subImage);
			cvZero(maskImage);

			CvPoint subP[4];
			for (int i = 0; i<4; ++i)
			{
				subP[i].x = boundaryPos[idx][i].x - p1.x;
				subP[i].y = boundaryPos[idx][i].y - p1.y;
			}
			cvFillConvexPoly(maskImage, subP, 4, cvScalarAll(255));

			cvSetImageROI(grayImage, r);
			cvCopy(grayImage, subImage, maskImage);
			cvResetImageROI(grayImage);
#if CVXFINDCONCENTRIBOARD_DEBUG
			cvShowImage("subImage", subImage);
			cvSaveImage("subImage.png", subImage);
			cvWaitKey(10);
#endif

			//computer circles center in sub-area
			Point2f seedP = Point2f(seedPos[idx].x - p1.x, seedPos[idx].y - p1.y);
			Point2f centerP;
			float ratio;
			float ratio_set = 0.5;
			
			bool isOK = CvxConcentric::detect(subImage, seedP, centerP, ratio, true, ratio_set, 0.005f);
			#if CVXFINDCONCENTRIBOARD_DEBUG
			fprintf(stdout, "ratio = %f\n", ratio);

			#endif
			if (isOK)
			{
				centerP.x += p1.x;
				centerP.y += p1.y;
				mask[idx] = true;
				pts[idx] = centerP;
			}
			else
			{
				mask[idx] = false;
				pts[idx] = Point2f(-1, -1);
			}
			
			cvReleaseImage(&subImage);
			cvReleaseImage(&maskImage);
		}
	}
	return true;
}

/************************************************************************/
/* 
*/
/************************************************************************/
CvxMarkerCCBoard::CvxMarkerCCBoard()
{
	m_size = cvSize(0, 0);
	m_unitLength = 0.0;  
	m_markerSize = cvSize(0, 0);
	m_markerOffset = cvSize(0, 0);
}
CvxMarkerCCBoard::~CvxMarkerCCBoard()
{

}

bool CvxMarkerCCBoard::SetConfigure(const CvSize &size, const float unitLength)
{
	CvSize markerSize = cvSize(size.width-3, size.height-3);
	CvSize markerOffset = cvSize(1, 1);
	assert(markerSize.width > markerOffset.width && markerSize.height > markerOffset.height);
	assert(markerSize.width + markerOffset.width <= size.width);
	assert(markerSize.height + markerOffset.height <= size.height);

	m_size = size;
	m_unitLength = unitLength;  
	m_markerSize = markerSize;
	m_markerOffset = markerOffset;
	for (int y = 0; y<m_size.height; ++y)
	{
		for (int x = 0; x<m_size.width; ++x)
		{
			Point2f p1;
			Point3f p2;
			p1.y = m_unitLength * y;
			p1.x = m_unitLength * x;
			p2.y = p1.y;
			p2.x = p1.x;
			p2.z = 0.0;
			m_pts2d.push_back(p1);
			m_pts3d.push_back(p2);
		}
	}
	return true;

}
bool CvxMarkerCCBoard::FindCorners(const IplImage *image, bool isTracking, const vector<CvxMarker>& preMarkers, vector<CvxMarker> &curMarker,
								   vector<Point2f> &pts2d, vector<Point3f> &pts3d)const
{
	assert(image);
	assert(pts2d.size() == 0);
	assert(pts3d.size() == 0);

	IplImage *grayImage = NULL;
	if (image->nChannels == 1)
	{
		grayImage = const_cast<IplImage *>(image);
	}
	else
	{
		grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
		cvCvtColor(image, grayImage, CV_BGR2GRAY);
	}

	vector<Point2f> pts;
	vector<bool> mask;
	bool isOk = cvxFindConcentriBoard(grayImage, 
		m_size, m_markerSize, 
		m_markerOffset, m_unitLength, pts,
		mask, isTracking, preMarkers, curMarker);
	if (isOk)
	{
		assert(pts.size() == mask.size() && pts.size() == m_pts3d.size());
		for (int i = 0; i<mask.size(); ++i)
		{
			if (mask[i])
			{
				pts2d.push_back(pts[i]);
				pts3d.push_back(m_pts3d[i]);
			}
		}

		//feature points threshold
		if (pts2d.size() >= 8)
		{
			return true;
		}		
	}
	pts2d.clear();
	pts3d.clear();
	if (grayImage != const_cast<IplImage *>(image))
	{
		cvReleaseImage(&grayImage);
	}
	return false;	
}

bool CvxMarkerCCBoard::FindCorners(const IplImage *image, bool isTracking, const vector<CvxMarker>& preMarkers, 
								   vector<CvxMarker> &curMarker, vector<Point2f> &pts2d)const
{
	return false;
}

bool CvxMarkerCCBoard::GetInsidePoints(const IplImage *image, vector<Point2f> &extremeCorners, 
									   vector<Point2f> &pts2d, vector<Point3f> &pts3d)const
{
	return true;
}

void CvxMarkerCCBoard::DrawCorners(const IplImage *image, const vector<Point2f> &pts)const
{
	for (int i = 0; i<pts.size(); ++i)
	{
		cv::circle(Mat(image), cvPoint(cvRound(pts[i].x), cvRound(pts[i].y)), 1, cvScalar(0, 0, 255), 1);
	}
}

/************************************************************************/
/* 
CvxMarkerInsiderChessboard
*/
/************************************************************************/

CvxMarkerInsiderChessboard::CvxMarkerInsiderChessboard()
{

}
CvxMarkerInsiderChessboard::~CvxMarkerInsiderChessboard()
{

}

bool CvxMarkerInsiderChessboard::SetConfigure(const CvSize &size, const float unitLength)
{
	assert(size.width > 5 && size.height>5);
	assert(unitLength > 0.0f);

	m_size = size;
	m_squareLen = unitLength;
	m_markerCenters.push_back(Point2f(unitLength, unitLength));
	m_markerCenters.push_back(Point2f(unitLength*(size.width-2), unitLength));
	m_markerCenters.push_back(Point2f(unitLength*(size.width-2), unitLength*(size.height-2)));
	m_markerCenters.push_back(Point2f(unitLength, unitLength*(size.height-2)));

	m_pts3d.resize(size.height*size.width);
	for (int y = 0; y<size.height; ++y)
	{
		for (int x = 0; x<size.width; ++x)
		{
			float f_x = x * unitLength;
			float f_y = y * unitLength;
			int idx = y * size.width + x;
			if ((x == 1 && y ==1) || 
				((x == size.width-2) && y == 1) ||
				((x == size.width-2) && (y == size.height-2)) ||
				((x == 1 && (y == size.height-2))))
			{
				m_pts2d.push_back(Point2f(0, 0));  //put (0, 0) instead of marker centers
			}
			else
			{
				m_pts2d.push_back(Point2f(f_x, f_y));
			}			
			m_pts3d[idx] = Point3f(f_x, f_y, 0.0);
		}
	}
	return true;

}
bool CvxMarkerInsiderChessboard::SetMarkerCorners(float markerLength)
{
	assert(m_markerCenters.size() == 4);
	assert(markerLength > 0);

	m_markerCorners3d.resize(4 * 4);
	float h_size = markerLength/2;
	for (int i = 0; i<m_markerCenters.size(); ++i)
	{
		Point2f p = m_markerCenters[i];
		m_markerCorners3d[4*i]   = Point3f(p.x - h_size, p.y - h_size, 0);
		m_markerCorners3d[4*i+1] = Point3f(p.x + h_size, p.y - h_size, 0);
		m_markerCorners3d[4*i+2] = Point3f(p.x + h_size, p.y + h_size, 0);
		m_markerCorners3d[4*i+3] = Point3f(p.x - h_size, p.y + h_size, 0);
	}
	return false;

}

bool CvxMarkerInsiderChessboard::FindCorners(const IplImage *image, bool isTracking, const vector<CvxMarker>& preMarkers, vector<CvxMarker> &curMarker,
											 vector<Point2f> &pts2d, vector<Point3f> &pts3d)const
{
	return false;
}

bool CvxMarkerInsiderChessboard::FindCorners(const IplImage *image, bool isTracking, const vector<CvxMarker>& preMarkers, 
											 vector<CvxMarker> &curMarker, vector<Point2f> &pts2d)const
{
	//check parameters
	assert(image);
	IplImage *grayImage = NULL;
	if (image->nChannels == 1)
	{
		grayImage = const_cast<IplImage*>(image);
	}
	else
	{
		grayImage = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
		assert(grayImage);
		cvCvtColor(image, grayImage, CV_BGR2GRAY);
	}
#define CvxMarkerInsiderChessboard_TEST 0
#if CvxMarkerInsiderChessboard_TEST 
	IplImage *showImage = cvCreateImage(cvGetSize(grayImage), IPL_DEPTH_8U, 3);
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);
	double tt = clock();	
#endif
	//get marker's position

	curMarker.clear();
	{
		MarkerCriteria criteria;
		criteria._minPixels = 100;
		criteria._maxPixels = 1200;
		criteria._fastCheck = true;
		int ret = CVX_MARKER_DIF_NUMBER;

		//tracking in four small image
		if (isTracking && preMarkers.size() == 4)
		{
			for (int i = 0; i<4; ++i)
			{
				CvRect r = preMarkers[i].m_subArea;
				IplImage *subImage = cvCreateImage(cvSize(r.width, r.height), IPL_DEPTH_8U, 1);
				cvSetImageROI(grayImage, r);
				cvCopy(grayImage, subImage);
				cvResetImageROI(grayImage);

				vector<CvxMarker> subMarker;

				int rst = CvxMarker::detect(subImage, subMarker, 1, criteria);			

				if (rst == CVX_MARKER_SUCCESS)
				{
					subMarker[0].AddOffset(r.x, r.y);
					curMarker.push_back(subMarker[0]);
				}
				else
				{
					//  					cvShowImage("subImage", subImage);
					//  					cvSaveImage("subImage.png", subImage);
					//  					cvWaitKey(0);
					cvReleaseImage(&subImage);
					curMarker.clear();
					break;
				}
				cvReleaseImage(&subImage);
			}
			if (curMarker.size() == 4)
			{
				ret = CVX_MARKER_SUCCESS;
			}
		}
		else
		{
			//detecting in whole image
			ret = CvxMarker::detect(grayImage, curMarker, 4, criteria);
		}		
		if (ret == CVX_MARKER_SUCCESS)
		{
			for (int i = 0; i<4; ++i)
			{
				curMarker[i].GetCenterPts();
				curMarker[i].GetSubArea(10, grayImage->width, grayImage->height);
			}
		}
		else
		{
			//	fprintf(stderr, "can not found markers!\n");
			if (grayImage != const_cast<IplImage*>(image))
			{
				cvReleaseImage(&grayImage);
			}
			return false;
		}
	}
#if CvxMarkerInsiderChessboard_TEST
	printf("marker tacking cost time = %f\n", clock() - tt);
	tt = clock();
#endif

	//get H
	assert(m_markerCenters.size() == 4);
	vector<Point2f> markerCenterVec;
	for (int i = 0; i<m_markerCenters.size(); ++i)
	{
		markerCenterVec.push_back(curMarker[i].m_centerPt);
	}
	Mat Homo = CvxSF::findHomography4Pts(m_markerCenters, markerCenterVec);

#if CvxMarkerInsiderChessboard_TEST 
	cvCvtColor(grayImage, showImage, CV_GRAY2BGR);

#endif
	//get chessboard corner position
	pts2d.resize(m_size.width * m_size.height);
	double *pData = (double*)(Homo.data);

	bool isAllInImage = true;
	for (int i = 0; i<m_pts2d.size(); ++i)
	{
		Point2f p1 = m_pts2d[i];
		Point2f p2;
		double scale = 1.0;
		p2.x  = pData[0] * p1.x  + pData[1] * p1.y + pData[2];
		p2.y  = pData[3] * p1.x  + pData[4] * p1.y + pData[5];
		scale = pData[6] * p1.x  + pData[7] * p1.y + pData[8];
		p2.x /= scale;
		p2.y /= scale;

		//make sure corner is not so near the image margin
		if (p2.x > 6 && p2.x < grayImage->width - 6 
			&& p2.y > 6 && p2.y < grayImage->height - 6)
		{
			pts2d[i] = p2;
		}
		else
		{
			isAllInImage = false;
			break;
		}		
	}
	if (isAllInImage)
	{
		cv::cornerSubPix(Mat(grayImage), pts2d, Size(5, 5), Size(-1, -1), 
			TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));

		//replace (1, 1)(w-2,1)(w-2,h-2)(1,h-2) with marker centers
		pts2d[1 * m_size.width + 1] = curMarker[0].m_centerPt;
		pts2d[1 * m_size.width + m_size.width - 2] = curMarker[1].m_centerPt;
		pts2d[(m_size.height-2) * m_size.width + m_size.width - 2] = curMarker[2].m_centerPt;
		pts2d[(m_size.height-2) * m_size.width + 1] = curMarker[3].m_centerPt;

	}
	else
	{
		pts2d.clear();
	}

#if CvxMarkerInsiderChessboard_TEST
	printf("sub pix cost time = %f\n", clock()-tt);
#endif

	if (grayImage != const_cast<IplImage*>(image))
	{
		cvReleaseImage(&grayImage);
	}
#if CvxMarkerInsiderChessboard_TEST 
	if (showImage)
	{
		cvReleaseImage(&showImage);
	}
#endif
	return isAllInImage;

}

bool CvxMarkerInsiderChessboard::GetInsidePoints(const IplImage *image, vector<Point2f> &extremeCorners, 
												 vector<Point2f> &pts2d, vector<Point3f> &pts3d)const
{
	return false;
}

void CvxMarkerInsiderChessboard::DrawCorners(const IplImage *image, const vector<Point2f> &pts)const
{
	assert(image);
	cv::drawChessboardCorners(Mat(image), m_size, pts, true);
}

float CvxMarkerInsiderChessboard::GetUnitLength(void)const
{
	return m_squareLen;
}
CvSize CvxMarkerInsiderChessboard::GetPatternSize(void)const
{
	return m_size;

}
void CvxMarkerInsiderChessboard:: Get3Dpts(vector<Point3f> &pts3d) const
{
	pts3d = m_pts3d;
}
